/*
 * 根据道路关键点（BLH deg,deg,m）
 * 转换成局部坐标（xyz m, m, m）
 * 输出地图文件 .map
 * */

#include "map_generator/map_generator.h"

namespace MAP_GEN{
    /* 读取文件：读入关键点 -BLH deg deg m */
    void Map_Generator::readFile(std::string file_name, vector<Map_Node::Ptr>& node_vec) {
        std::ifstream fp;
        std::string _file = file_path_ + file_name;
        fp.open(_file);
        if(!fp){
            std::cerr << "open the file [" << file_name << "] error ! " << std::endl;
            fp.close();
            exit(-1);
        }

        vector<Map_Node::Ptr>().swap(node_vec);

        std::string line;
        double B, L, H;
        Eigen::Vector3d BLH, pos;
        while(getline(fp, line)){
            if(getLineHeader(line) == "$latlon"){
                B = getFirstNumber(line);
                L = getFirstNumber(line);
                H = getFirstNumber(line);

                BLH << B*D2R, L*D2R, H;
                Map_Node::Ptr node = std::make_shared<Map_Node>(BLH);
                //BLH(deg deg m)转换为局部坐标xyz (m m m)
                blh2Locpos_.BLH2xy(BLH, pos);
                node->setLocpos(pos);

                node_vec.push_back(node);
            }
        }
    }
    /* 获取文件一行最开始的数字，并删去数字 */
    double Map_Generator::getFirstNumber(std::string &line) {
        double result;

        std::size_t index = line.find_first_of(',');
        if(index != std::string::npos){
            result = atof(line.substr(0, index).c_str());
            line   = line.substr(index+1, line.size()-index-1);
        } else{
            result = atof(line.c_str());
        }

        return result;
    }
    /* 获取文件一行的前缀，并删去前缀 */
    std::string Map_Generator::getLineHeader(std::string &line) {
        std::size_t index  = line.find_first_of(',');
        std::string header = line.substr(0, index);
        line = line.substr(index+1, line.size()-index-1);

        return header;
    }
    /* 合并 范型 */
    template <typename T>
    void Map_Generator::vectorMerge(vector<T> &A, vector<T> &B) {
        if(A.empty())
            A.swap(B);
        else
            for(auto& b : B)
                A.push_back(b);

    }

    void Map_Generator::run() {
        vector<Map_Node::Ptr> node_vec;
        readFile(fileA_, node_vec);
        vectorMerge(graph_, node_vec);

        readFile(fileB_, node_vec);
        vectorMerge(graph_, node_vec);

        readFile(fileC_, node_vec);
        vectorMerge(graph_, node_vec);

        readFile(fileD_, node_vec);
        vectorMerge(graph_, node_vec);

        createRoad();
        createMap();
        writeFile();
    }

    /* 确定关键点的连接关系，连接的边即为道路 */
    void Map_Generator::createRoad() {
        //1号教学楼到友谊广场区域
        //友谊广场下面的停车场
        createEdge(graph_[0], graph_[2]);
        createEdge(graph_[0], graph_[1]);
        createEdge(graph_[1], graph_[3]);
        //停车场向中心（南）
        createEdge(graph_[2], graph_[7]);
        for (int i = 7; i < 10; ++i)
            createEdge(graph_[i], graph_[i+1]);
        //停车场向中心（北）
        createEdge(graph_[4], graph_[12]);
        createEdge(graph_[11], graph_[12]);
        createEdge(graph_[11], graph_[14]);
        //中心到测绘到附2
        createEdge(graph_[10], graph_[13]);
        createEdge(graph_[13], graph_[15]);
        //青楼南（南、东）
        createEdge(graph_[14], graph_[17]);
        createEdge(graph_[4], graph_[17]);
        //青楼南（北）
        createEdge(graph_[16], graph_[19]);
        createEdge(graph_[20], graph_[19]);
        //停车场到遥感(南)
        createEdge(graph_[3], graph_[24]);
        for (int i = 24; i < 28; ++i)
            createEdge(graph_[i], graph_[i+1]);
        //青楼正门前
        createEdge(graph_[5], graph_[18]);
        //青楼南(南、南西)
        createEdge(graph_[23], graph_[18]);
        //停车场到遥感（北）
        createEdge(graph_[6], graph_[5]);
        createEdge(graph_[6], graph_[23]);
        //遥感到青楼门洞
        createEdge(graph_[28], graph_[22]);
        createEdge(graph_[21], graph_[22]);
        //门洞（西、食堂）
        createEdge(graph_[21], graph_[32]);
        createEdge(graph_[31], graph_[32]);
//        createEdge(graph_[31], graph_[30]);
        createEdge(graph_[29], graph_[20]);
        //门洞（东、通道）
        createEdge(graph_[16], graph_[38]);
        createEdge(graph_[15], graph_[37]);
        //青楼北（国重往操场）
        createEdge(graph_[29], graph_[35]);
        createEdge(graph_[30], graph_[34]);
        createEdge(graph_[33], graph_[34]);
        createEdge(graph_[35], graph_[36]);
        //门洞（东）往操场
        createEdge(graph_[38], graph_[36]);
        createEdge(graph_[39], graph_[33]);
        //东门（东）往体育馆
        createEdge(graph_[40], graph_[43]);
        createEdge(graph_[42], graph_[43]);
        createEdge(graph_[42], graph_[41]);
        createEdge(graph_[37], graph_[41]);

//        createEdge(graph_[39], graph_[40]);

        //友谊广场
        for (int i = 44; i < 47; ++i)
            createEdge(graph_[i], graph_[i+1]);
        createEdge(graph_[44], graph_[47]);
        for (int i = 48; i < 73; ++i)
            createEdge(graph_[i], graph_[i+1]);
        createEdge(graph_[48], graph_[73]);

        //图书馆到1号教学楼区域
        createEdge(graph_[74], graph_[75]);
        for(int i = 76; i < 79; i++)
            createEdge(graph_[i], graph_[i+1]);
        createEdge(graph_[80], graph_[81]);
        for(int i = 82; i < 84; i++)
            createEdge(graph_[i], graph_[i+1]);
        createEdge(graph_[84], graph_[93]);
        createEdge(graph_[82], graph_[93]);
        for(int i = 85; i < 88; i++)
            createEdge(graph_[i], graph_[i+1]);
        createEdge(graph_[85], graph_[88]);

        createEdge(graph_[74], graph_[89]);
        createEdge(graph_[89], graph_[40]);

        createEdge(graph_[81], graph_[31]);

        createEdge(graph_[39], graph_[90]);
        for(int i = 90; i < 92; i++)
            createEdge(graph_[i], graph_[i+1]);
        createEdge(graph_[92], graph_[30]);

        //图书馆地下停车场
        createEdge(graph_[75], graph_[94]);
        createEdge(graph_[76], graph_[95]);
        createEdge(graph_[79], graph_[106]);
        createEdge(graph_[80], graph_[105]);

        createEdge(graph_[94], graph_[96]);
        createEdge(graph_[95], graph_[97]);
        createEdge(graph_[97], graph_[98]);
        createEdge(graph_[99], graph_[100]);
        createEdge(graph_[98], graph_[101]);
        createEdge(graph_[101], graph_[102]);
        createEdge(graph_[102], graph_[99]);
        createEdge(graph_[100], graph_[103]);
        createEdge(graph_[96], graph_[104]);
        createEdge(graph_[103], graph_[106]);
        createEdge(graph_[104], graph_[105]);
    }

    /* 构建 node1 与 node2 之间的边，并将两节点互为相邻节点 */
    void Map_Generator::createEdge(Map_Node::Ptr &node1, Map_Node::Ptr &node2) {
        Edge::Ptr edge = std::make_shared<Edge>(node1, node2);

        node1->addNeighbor(node2);
        node1->edges_.push_back(edge);
        node2->addNeighbor(node1);
        node2->edges_.push_back(edge);
    }

    /* 生成地图 */
    void Map_Generator::createMap() {
        vector<Eigen::Vector3d> obstacle;
        getObstaclePoint(obstacle);
        /*设置参数*/
        double &xmin   = mapparam_.xmin_;
        double &xmax   = mapparam_.xmax_;
        double &ymin   = mapparam_.ymin_;
        double &ymax   = mapparam_.ymax_;
        double &margin = mapparam_.map_margin_;
        double xsize = xmax - xmin + 2 * margin;
        double ysize = ymax - ymin + 2 * margin;
        double zsize = mapparam_.resolution_;
        /*创建地图*/
        Eigen::Vector3d map_origin(xmin - margin, ymin - margin, 0);
        Eigen::Translation3d origin_translation(map_origin[0], map_origin[1], map_origin[2]);
        Eigen::Quaterniond origin_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Affine3d origin_transform = origin_translation * origin_rotation;
        collision_map_.reset(new sdf_tools::CollisionMapGrid(origin_transform, "world", mapparam_.resolution_,
                                                             xsize, ysize, zsize, free_cell_));
        /*设置障碍物栅格*/
        for(auto& p : obstacle)
            collision_map_->Set(p[0], p[1], p[2], obst_cell_);

    }

    /* 将关键点相连
     * 分辨率 0.2m/栅格
     * 输出视为障碍物的道路的边的点的序列 */
    void Map_Generator::getObstaclePoint(vector<Eigen::Vector3d> &obstacle) {
        /*记录连接边的关键点，用于android平板人机交互的地图显示*/
        std::ofstream BLHf;
        BLHf.open(file_path_ + "whu_map.txt");
        if(!BLHf) {
            std::cerr << "open result file [whu_map.txt] is error !" << std::endl;
            exit(-1);
        }
        /*记录连接边的关键点，用于android平板人机交互的地图显示*/

        for(auto& node1 : graph_){
            if(node1->state_ == STATE::close)
                continue;
            if(node1->state_ == STATE::init){
                node1->state_ = STATE::close;

                for(auto& node2 : node1->neighbors_){
                    if(node2->state_ == STATE::close)
                        continue;

                    BLHf << getBLHString(node1, node2) << std::endl;
                    auto& p1 = node1->loc_pos_;
                    auto& p2 = node2->loc_pos_;
                    mapparam_.getMapSize(p1, p2);
                    /*将边离散化*/
                    double length = (p1 - p2).norm();
                    int    num = ceil(length/(double)mapparam_.resolution_);

                    for(int i = 0; i <= num; i++){
                        Eigen::Vector3d _p;
                        _p = i/(double)num * p1 + (num - i)/(double)num * p2;
                        obstacle.push_back(_p);
                    }
                }
            }
        }

        BLHf.close();
    }

    std::string Map_Generator::getBLHString(Map_Node::Ptr& node1, Map_Node::Ptr& node2) {
        std::string result = "$BLH,";
        auto& p1 = node1->pos_BLH_;
        std::string s1 = double2string(p1[0] * R2D) + ","
                + double2string(p1[1] * R2D) + ","
                + double2string(p1[2]) + ",";

        result += s1;
        auto& p2 = node2->pos_BLH_;
        std::string s2 = double2string(p2[0] * R2D) + ","
                         + double2string(p2[1] * R2D) + ","
                         + double2string(p2[2]);
        result += s2;

        return result;
    }

    /*制作地图文件*/
    void Map_Generator::writeFile() {
        std::string _file = file_path_ + file_result_;
        std::ofstream wf;

        wf.open(_file);
        if(!wf){
            std::cerr << "open result file [" << file_result_ << "] is error !" << std::endl;
            wf.close();
            exit(-1);
        }

        wf << "$resolution," << mapparam_.resolution_ << std::endl;
        wf << "$original_point,30.528628340918,114.35572874791,22.813257" << std::endl;
        wf << "$mapsize," << mapparam_.xmin_ << ","
        << mapparam_.xmax_ << ","
        << mapparam_.ymin_ << ","
        << mapparam_.ymax_ << std::endl;
        for (int64_t x = 0; x < collision_map_->GetNumXCells(); x++)
            for (int64_t y = 0; y < collision_map_->GetNumYCells(); y++){
                auto cell = collision_map_->Get(x, y, 0);
                if (cell.first.occupancy > 0.5) {
                    wf << "$obstacle," << x << "," << y
                        << "," << cell.first.occupancy << std::endl;
                    auto pos = collision_map_->GridIndexToLocation(x, y, 0);
                    wf << "$real_xy," << pos[0] << "," << pos[1] << "," << pos[2] << std::endl;
                }
            }

        wf.close();
        std::cout << "write map file success !" << std::endl;
    }
}

int main(int argc, char** argv){
    MAP_GEN::Map_Generator map_generator;
    std::cout << "[wmr] map generate " << std::endl;
    map_generator.run();

    return 0;
}